/***************************************************************************//**
 * @file GUI/DialogTest.cpp
 * @author  Marek M. Cel <marekcel@mscsim.org>
 *
 * @section LICENSE
 *
 * Copyright (C) 2013 Marek M. Cel
 *
 * This file is part of MScSim. You can redistribute and modify it under
 * the terms of GNU General Public License as published by the Free Software
 * Foundation; either version 3 of the License, or (at your option) any later
 * version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
 * for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.
 * 59 Temple Place - SpUite 330, Boston, MA  02111-1307, USA.
 *
 * Further information about the GNU General Public License can also be found
 * on the world wide web at http://www.gnu.org.
 ******************************************************************************/
#ifndef DIALOG_TEST_CPP
#define DIALOG_TEST_CPP
#endif

////////////////////////////////////////////////////////////////////////////////

#ifndef _USE_MATH_DEFINES
#   define _USE_MATH_DEFINES
#endif

////////////////////////////////////////////////////////////////////////////////

#include <iostream>

#ifdef WIN32
    #define NOMINMAX
    #include <windows.h>
#endif

#ifdef _LINUX_
    #include <arpa/inet.h>
#endif

#include <Utils.h>

#include <utils/MSc_Angles.h>
#include <utils/MSc_Location.h>
#include <utils/MSc_Matrix3x3.h>

#include <utils/MSc_XmlDocument.h>

#include <GUI/DialogTest.h>
#include <ui_DialogTest.h>

////////////////////////////////////////////////////////////////////////////////

using namespace std;

////////////////////////////////////////////////////////////////////////////////

void htond( double &x )
{
    int *ptr = (int*)(&x);
    int  tmp = ptr[ 0 ];

    ptr[ 0 ] = htonl( ptr[ 1 ] );
    ptr[ 1 ] = htonl( tmp );
}

////////////////////////////////////////////////////////////////////////////////

void htonf( float &x )
{
    int *ptr = (int*)(&x);
    int  tmp = ptr[ 0 ];

    ptr[ 0 ] = htonl( tmp );
}

////////////////////////////////////////////////////////////////////////////////

void recurentFunc( MSc_XmlNode &node )
{
    if ( node.isElement() )
    {
        cout << "<" << node.getName();

        if ( node.hasAttributes() )
        {
            MSc_XmlNode::Attributes attributes = node.getAttributes();

            MSc_XmlNode::Attributes::iterator it = attributes.begin();

            while ( it != attributes.end() )
            {
                cout << " " << it->first << "=\"" << it->second << "\"";

                ++it;
            }
        }

        cout << ">";

        if ( node.hasChildren() )
        {
            MSc_XmlNode child = node.getFirstChild();

            while ( child.isValid() )
            {
                recurentFunc( child );

                child = child.getNextSibling();
            }
        }

        cout << "</" << node.getName() << ">";
    }
    else
    {
        if ( node.isText() )
        {
            cout << node.getText();// << endl;
        }
    }
}

////////////////////////////////////////////////////////////////////////////////

DialogTest::DialogTest( QWidget *parent ) :
    QDialog( parent ),
    m_ui( new Ui::DialogTest ),

    m_data ( new GUI_Data() ),

    m_sockCtr ( 0 ),
    m_sockFdm ( 0 ),

    m_dataFromFGFS ( false )
{
    m_ui->setupUi( this );

    memset( m_data, 0, sizeof(GUI_Data) );

    m_sockCtr = new QUdpSocket();
    m_sockFdm = new QUdpSocket();

    memset( &m_netCtr, 0, sizeof(FGNetCtrls) );
    memset( &m_netFdm, 0, sizeof(FGNetFDM)   );

    m_sockCtr->connectToHost( QHostAddress( "127.0.0.1" ), (quint16)5501 );
    m_sockFdm->bind( QHostAddress( "127.0.0.1" ), (quint16)5500, QUdpSocket::ShareAddress );

    //////////////////////////////////

    MSc_XmlDocument *doc = new MSc_XmlDocument();

    if ( MSCFDM_SUCCESS == doc->readFile( "data/aircrafts/aircraft_types.xml" ) )
    {
        cout << "SUCCESS"  << endl;

        MSc_XmlNode root = doc->getRootNode();

        recurentFunc( root );
        cout << endl;
    }
    else
    {
        cout << "FAILURE"  << endl;
    }
    //////////////////////////////////
}

////////////////////////////////////////////////////////////////////////////////

DialogTest::~DialogTest()
{
    if ( m_sockCtr )
    {
        m_sockCtr->abort();
        m_sockCtr->close();
        delete m_sockCtr; m_sockCtr = 0;
    }

    if ( m_sockFdm )
    {
        m_sockFdm->abort();
        m_sockFdm->close();
        delete m_sockFdm; m_sockFdm = 0;
    }

    if ( m_ui ) delete m_ui; m_ui = 0;

    ////////////////////////////////////////
    if ( m_data ) delete m_data; m_data = 0;
    ////////////////////////////////////////
}

////////////////////////////////////////////////////////////////////////////////

void DialogTest::update( double /*timeStep*/ )
{
    if ( m_sockFdm->state() == QAbstractSocket::BoundState )
    {
        while ( m_sockFdm->hasPendingDatagrams() )
        {
            receiveDataFromFGFS();
        }
    }

    if ( m_dataFromFGFS )
    {
        dataFromFGFS();
    }
    else
    {
        dataFromGUI();
    }

    m_data->dataInp.cgiDataInp.dateTime.year   = m_ui->dateTimeEdit->date().year();
    m_data->dataInp.cgiDataInp.dateTime.month  = m_ui->dateTimeEdit->date().month();
    m_data->dataInp.cgiDataInp.dateTime.day    = m_ui->dateTimeEdit->date().day();
    m_data->dataInp.cgiDataInp.dateTime.hour   = m_ui->dateTimeEdit->time().hour();
    m_data->dataInp.cgiDataInp.dateTime.minute = m_ui->dateTimeEdit->time().minute();
    m_data->dataInp.cgiDataInp.dateTime.second = m_ui->dateTimeEdit->time().second();
}

////////////////////////////////////////////////////////////////////////////////

void DialogTest::updateDataInp( GUI_DataInp &dataInp )
{
    dataInp = m_data->dataInp;
}

////////////////////////////////////////////////////////////////////////////////

void DialogTest::updateDataOut( GUI_DataOut &dataOut )
{
    dataOut = m_data->dataOut;
}

////////////////////////////////////////////////////////////////////////////////

void DialogTest::on_pushButtonFGFS_Off_clicked()
{
    m_dataFromFGFS = false;
    m_ui->groupAircraftData->setDisabled( false );
}

////////////////////////////////////////////////////////////////////////////////

void DialogTest::on_pushButtonFGFS_On_clicked()
{
    m_dataFromFGFS = true;
    m_ui->groupAircraftData->setDisabled( true );
}

////////////////////////////////////////////////////////////////////////////////

void DialogTest::dataFromGUI()
{
    memset( &m_data->dataInp, 0, sizeof(GUI_DataInp) );

    double lat = Utils::deg2rad( m_ui->spinLat->value() );
    double lon = Utils::deg2rad( m_ui->spinLon->value() );
    double alt = m_ui->spinAlt->value();
    double phi = Utils::deg2rad( m_ui->spinPhi->value() );
    double tht = Utils::deg2rad( m_ui->spinTht->value() );
    double psi = Utils::deg2rad( m_ui->spinPsi->value() );

    m_data->dataInp.cgiDataInp.aircraft.latitude  = lat;
    m_data->dataInp.cgiDataInp.aircraft.longitude = lon;
    m_data->dataInp.cgiDataInp.aircraft.altitude  = alt;

    MSc_Location location = MSc_Location::getFromGeo( lat, lon, alt );

    MSc_Angles  attitude = MSc_Angles::getFrom( phi, tht, psi );

    MSc_Matrix3x3 T_wgs2ned = location.getT_wgs2ned();
    MSc_Matrix3x3 T_ned2bas = MSc_Matrix3x3::getRotationMatrix( attitude );
    MSc_Matrix3x3 T_wgs2bas = T_ned2bas * T_wgs2ned;

    double s_tht = -T_wgs2bas(0,2);
    double c_tht = sqrt( 1.0 - min( 1.0, s_tht*s_tht) );

    m_data->dataInp.cgiDataInp.aircraft.wgs_tht = atan2( s_tht, c_tht );

    if ( c_tht > 0.0 )
    {
        m_data->dataInp.cgiDataInp.aircraft.wgs_phi = atan2( T_wgs2bas(1,2) , T_wgs2bas(2,2) );
        m_data->dataInp.cgiDataInp.aircraft.wgs_psi = atan2( T_wgs2bas(0,1) , T_wgs2bas(0,0) );
    }
    else
    {
        m_data->dataInp.cgiDataInp.aircraft.wgs_phi = atan2( -T_wgs2bas(1,0) , -T_wgs2bas(2,0) );
        m_data->dataInp.cgiDataInp.aircraft.wgs_psi = 0.0;
    }

    m_data->dataInp.cgiDataInp.aircraft.wgs_x = location.getX();
    m_data->dataInp.cgiDataInp.aircraft.wgs_y = location.getY();
    m_data->dataInp.cgiDataInp.aircraft.wgs_z = location.getZ();

    /////////////

    m_data->dataInp.flightData.altitude_asl = alt;
    m_data->dataInp.flightData.altitude_agl = alt;
}

////////////////////////////////////////////////////////////////////////////////

void DialogTest::dataFromFGFS()
{
    m_data->dataInp.cgiDataInp.aircraft.latitude  = m_netFdm.latitude;
    m_data->dataInp.cgiDataInp.aircraft.longitude = m_netFdm.longitude;
    m_data->dataInp.cgiDataInp.aircraft.altitude  = m_netFdm.altitude;

    MSc_Location location = MSc_Location::getFromGeo( m_netFdm.latitude,
                                                      m_netFdm.longitude,
                                                      m_netFdm.altitude );

    MSc_Angles  attitude = MSc_Angles::getFrom( m_netFdm.phi,
                                                m_netFdm.theta,
                                                m_netFdm.psi );

    MSc_Matrix3x3 T_wgs2ned = location.getT_wgs2ned();
    MSc_Matrix3x3 T_ned2bas = MSc_Matrix3x3::getRotationMatrix( attitude );
    MSc_Matrix3x3 T_wgs2bas = T_ned2bas * T_wgs2ned;

    double s_tht = -T_wgs2bas(0,2);
    double c_tht = sqrt( 1.0 - min( 1.0, s_tht*s_tht ) );

    m_data->dataInp.cgiDataInp.aircraft.wgs_tht = atan2( s_tht, c_tht );

    if ( c_tht > 0.0 )
    {
        m_data->dataInp.cgiDataInp.aircraft.wgs_phi = atan2( T_wgs2bas(1,2) , T_wgs2bas(2,2) );
        m_data->dataInp.cgiDataInp.aircraft.wgs_psi = atan2( T_wgs2bas(0,1) , T_wgs2bas(0,0) );
    }
    else
    {
        m_data->dataInp.cgiDataInp.aircraft.wgs_phi = atan2( -T_wgs2bas(1,0) , -T_wgs2bas(2,0) );
        m_data->dataInp.cgiDataInp.aircraft.wgs_psi = 0.0;
    }

    m_data->dataInp.cgiDataInp.aircraft.wgs_x = location.getX();
    m_data->dataInp.cgiDataInp.aircraft.wgs_y = location.getY();
    m_data->dataInp.cgiDataInp.aircraft.wgs_z = location.getZ();

    /////////////
    m_data->dataInp.flightData.altitude_asl = m_netFdm.altitude;
    m_data->dataInp.flightData.altitude_agl = m_netFdm.agl;

    m_data->dataInp.flightData.airspeed   = MSc_Utils::kts2mps( m_netFdm.vcas );
    m_data->dataInp.flightData.climbRate  = MSc_Utils::fps2mps( m_netFdm.climb_rate );
    m_data->dataInp.flightData.machNumber = 0.0;

    m_data->dataInp.flightData.rollAngle  = m_netFdm.phi;
    m_data->dataInp.flightData.pitchAngle = m_netFdm.theta;
    m_data->dataInp.flightData.heading    = m_netFdm.psi;

    m_data->dataInp.flightData.angleOfAttack = m_netFdm.alpha;
    m_data->dataInp.flightData.sideslipAngle = m_netFdm.beta;

    m_data->dataInp.flightData.course    = 0.0;
    m_data->dataInp.flightData.pathAngle = 0.0;

    m_data->dataInp.flightData.slipSkidAngle = 0.0;

    m_data->dataInp.flightData.rollRate  = m_netFdm.phidot;
    m_data->dataInp.flightData.pitchRate = m_netFdm.thetadot;
    m_data->dataInp.flightData.yawRate   = m_netFdm.psidot;
    m_data->dataInp.flightData.turnRate  = 0.0;

    m_data->dataInp.flightData.Gx = m_netFdm.A_X_pilot / 32.17;
    m_data->dataInp.flightData.Gy = m_netFdm.A_Y_pilot / 32.17;
    m_data->dataInp.flightData.Gz = m_netFdm.A_Z_pilot / 32.17;

    ////////////////////////////////

    m_ui->spinLon->setValue( Utils::rad2deg( m_netFdm.longitude ) );
    m_ui->spinLat->setValue( Utils::rad2deg( m_netFdm.latitude  ) );
    m_ui->spinAlt->setValue( m_netFdm.altitude );
    m_ui->spinPsi->setValue( Utils::rad2deg( m_netFdm.psi   ) );
    m_ui->spinTht->setValue( Utils::rad2deg( m_netFdm.theta ) );
    m_ui->spinPhi->setValue( Utils::rad2deg( m_netFdm.phi   ) );
}

////////////////////////////////////////////////////////////////////////////////

void DialogTest::receiveDataFromFGFS()
{
    FGNetFDM netFdm;

    qint64 receivedDataSize = m_sockFdm->readDatagram( (char*)&netFdm, (qint64)sizeof(FGNetFDM) );

    if ( receivedDataSize == (qint64)sizeof(FGNetFDM) )
    {
        netFdm.version = htonl( netFdm.version );
        netFdm.padding = htonl( netFdm.padding );

        //cout << netFdm.version << endl;

        if ( netFdm.version == FG_NET_FDM_VERSION )
        {
            m_netFdm = netFdm;
            htond( m_netFdm.longitude );
            htond( m_netFdm.latitude  );
            htond( m_netFdm.altitude  );
            htonf( m_netFdm.agl       );
            htonf( m_netFdm.phi       );
            htonf( m_netFdm.theta     );
            htonf( m_netFdm.psi       );
            htonf( m_netFdm.alpha     );
            htonf( m_netFdm.beta      );

            htonf( m_netFdm.phidot            );
            htonf( m_netFdm.thetadot          );
            htonf( m_netFdm.psidot            );
            htonf( m_netFdm.vcas              );
            htonf( m_netFdm.climb_rate        );
            htonf( m_netFdm.v_north           );
            htonf( m_netFdm.v_east            );
            htonf( m_netFdm.v_down            );
            htonf( m_netFdm.v_wind_body_north );
            htonf( m_netFdm.v_wind_body_east  );
            htonf( m_netFdm.v_wind_body_down  );

            htonf( m_netFdm.A_X_pilot );
            htonf( m_netFdm.A_Y_pilot );
            htonf( m_netFdm.A_Z_pilot );

            htonf( m_netFdm.stall_warning );
            htonf( m_netFdm.slip_deg      );

            m_netFdm.num_engines = htonl( m_netFdm.num_engines );
            for ( int i = 0; i < FGNetFDM::FG_MAX_ENGINES; i++ )
            {
                m_netFdm.eng_state[ i ] = htonl( m_netFdm.eng_state[ i ] );
                htonf( m_netFdm.rpm       [ i ] );
                htonf( m_netFdm.fuel_flow [ i ] );
                htonf( m_netFdm.fuel_px   [ i ] );
                htonf( m_netFdm.egt       [ i ] );
                htonf( m_netFdm.cht       [ i ] );
                htonf( m_netFdm.mp_osi    [ i ] );
                htonf( m_netFdm.tit       [ i ] );
                htonf( m_netFdm.oil_temp  [ i ] );
                htonf( m_netFdm.oil_px    [ i ] );
            }

            m_netFdm.num_tanks = htonl( m_netFdm.num_tanks );
            for ( int i = 0; i < FGNetFDM::FG_MAX_TANKS; i++ )
            {
                htonf( m_netFdm.fuel_quantity[ i ] );
            }

            m_netFdm.num_wheels = htonl( m_netFdm.num_wheels );
            for ( int i = 0; i < FGNetFDM::FG_MAX_WHEELS; i++ )
            {
                m_netFdm.wow[i] = htonl( m_netFdm.wow[ i ] );
                htonf( m_netFdm.gear_pos         [ i ] );
                htonf( m_netFdm.gear_steer       [ i ] );
                htonf( m_netFdm.gear_compression [ i ] );
            }

            m_netFdm.cur_time = htonl( m_netFdm.cur_time );
            m_netFdm.warp     = htonl( m_netFdm.warp     );
            htonf( m_netFdm.visibility );

            htonf( m_netFdm.elevator          );
            htonf( m_netFdm.elevator_trim_tab );
            htonf( m_netFdm.left_flap         );
            htonf( m_netFdm.right_flap        );
            htonf( m_netFdm.left_aileron      );
            htonf( m_netFdm.right_aileron     );
            htonf( m_netFdm.rudder            );
            htonf( m_netFdm.nose_wheel        );
            htonf( m_netFdm.speedbrake        );
            htonf( m_netFdm.spoilers          );
        }
        else
        {
            cout << __FILE__ << "(" << __LINE__ << ")" << endl;
            cout << "Wrong NET_FDM data package version." << endl;
            cout << "   Expected: " << FG_NET_FDM_VERSION << endl;
            cout << "   Received: " << m_netFdm.version   << endl;
        }
    }
    else
    {
        cout << __FILE__ << "(" << __LINE__ << ")" << endl;
        cout << "Wrong NET_FDM data package size." << endl;
        cout << "   Expected: " << (qint64)sizeof(FGNetFDM) << endl;
        cout << "   Received: " << receivedDataSize         << endl;
    }
}
